﻿#include "stdafx.h"
#include "sbcs.h"
#if _SB_X86_
#include <emmintrin.h>
#define SB_MM2
#if !defined(_SB_64_) && defined(_MSC_VER)
#define SB_MATH_ASM
#endif
#endif
#include "sbmath.h"

//////////////////////////////////////////////////////////////////////////
// 인트린식 전용
#ifdef SB_MM2
#define MMALIGN K_ALIGN(16)

static const MMALIGN kint _k_mt_im_NNNN[4] = {0x80000000, 0x80000000, 0x80000000, 0x80000000};
static const MMALIGN kint _k_mt_im_PNPN[4] = {0x00000000, 0x80000000, 0x00000000, 0x80000000};
static const MMALIGN kint _k_mt_im_NPNP[4] = {0x80000000, 0x00000000, 0x80000000, 0x00000000};
static const MMALIGN kint _k_mt_im_PNNP[4] = {0x00000000, 0x80000000, 0x80000000, 0x00000000};
static const MMALIGN kint _k_mt_im_0FFF[4] = {0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000};
static const MMALIGN float _k_mt_im_1001[4] = {1.0f, 0.0f, 0.0f, 1.0f};
static const MMALIGN float _k_mt_im_1000[4] = {1.0f, 0.0f, 0.0f, 0.0f};
static const MMALIGN float _k_mt_im_0000[4] = {0.0f, 0.0f, 0.0f, 0.0f};

#define _K_MM_NNNN			(*(__m128*)&_k_mt_im_NNNN)		// - - - -
#define _K_MM_PNPN			(*(__m128*)&_k_mt_im_PNPN)		// + - + -
#define _K_MM_NPNP			(*(__m128*)&_k_mt_im_NPNP)		// - + - +
#define _K_MM_PNNP			(*(__m128*)&_k_mt_im_PNNP)		// + - - +
#define _K_MM_0FFF			(*(__m128*)&_k_mt_im_0FFF)		// 0 * * *
#define _K_MM_1001			(*(__m128*)&_k_mt_im_1001)		// 1 0 0 1
#define _K_MM_1000			(*(__m128*)&_k_mt_im_1000)		// 1 0 0 0
#define _K_MM_0000			(*(__m128*)&_k_mt_im_0000)		// 0 0 0 0

#define _mm_ror_ps(vec,i)   (((i)%4) ? (_mm_shuffle_ps(vec,vec, _MM_SHUFFLE((unsigned char)(i+3)%4,(unsigned char)(i+2)%4,(unsigned char)(i+1)%4,(unsigned char)(i+0)%4))) : (vec))
#define _mm_rol_ps(vec,i)   (((i)%4) ? (_mm_shuffle_ps(vec,vec, _MM_SHUFFLE((unsigned char)(7-i)%4,(unsigned char)(6-i)%4,(unsigned char)(5-i)%4,(unsigned char)(4-i)%4))) : (vec))
#define _mm_abs_ps(vec)     _mm_andnot_ps(_K_MM_NNNN,vec)
#define _mm_neg_ps(vec)     _mm_xor_ps(_K_MM_NNNN,vec)
#endif

#ifdef SB_MATH_ASM
static const float _k_mt_float_zero = 0.0f;
#endif


//////////////////////////////////////////////////////////////////////////
// 일반

// 사인코사인메인
#ifdef SB_MATH_ASM
K_INLINE void k_sincos_main(float f, float *s, float* c)
{
	__asm
	{
		mov eax, s;
		mov edx, c;
		fld f;
		fsincos;
		fstp DWORD ptr[edx];
		fstp DWORD ptr[eax];
		fwait;
	}
}
#endif

// 사인코사인
void k_sincos(float f, float* s, float* c)
{
#ifdef SB_MATH_ASM
	k_sincos_main(f, s, c);
#else
	*s=k_sin(f);
	*c=k_cos(f);
#endif
}


//////////////////////////////////////////////////////////////////////////
// 벡터2

// 정규화
void k_vec2_norm(kVec2* pv, const kVec2* v)
{
	float f = k_vec2_len(v);
	if (f > 0.0f) f = 1.0f / f;
	pv->x = v->x*f;
	pv->y = v->y*f;
}

// 바리 센트릭
void k_vec2_bary_centric(kVec2* pv, const kVec2* v1, const kVec2* v2, const kVec2* v3, float f, float g)
{
	pv->x = (1.0f - f - g)*(v1->x) + f*(v2->x) + g*(v3->x);
	pv->y = (1.0f - f - g)*(v1->y) + f*(v2->y) + g*(v3->y);
}

// 캐트멀롬
void k_vec2_catmull_rom(kVec2* pv, const kVec2* v0, const kVec2* v1, const kVec2* v2, const kVec2* v3, float s)
{
	pv->x = 0.5f*(2.0f*v1->x + (v2->x - v0->x)*s + (2.0f*v0->x - 5.0f*v1->x + 4.0f*v2->x - v3->x)*s*s + (v3->x - 3.0f*v2->x + 3.0f*v1->x - v0->x)*s*s*s);
	pv->y = 0.5f*(2.0f*v1->y + (v2->y - v0->y)*s + (2.0f*v0->y - 5.0f*v1->y + 4.0f*v2->y - v3->y)*s*s + (v3->y - 3.0f*v2->y + 3.0f*v1->y - v0->y)*s*s*s);
}

// 허밋
void k_vec2_hermite(kVec2* pv, const kVec2* v1, const kVec2* t1, const kVec2* v2, const kVec2* t2, float s)
{
	float h1 = 2.0f*s*s*s - 3.0f*s*s + 1.0f;
	float h2 = s*s*s - 2.0f*s*s + s;
	float h3 = -2.0f*s*s*s + 3.0f*s*s;
	float h4 = s*s*s - s*s;

	pv->x = h1*v1->x + h2*t1->x + h3*v2->x + h4*t2->x;
	pv->y = h1*v1->y + h2*t1->y + h3*v2->y + h4*t2->y;
}


//////////////////////////////////////////////////////////////////////////
// 벡터3

// 단위벡터
void k_vec3_norm(kVec3* pv, const kVec3* v)
{
#ifdef SB_MATH_ASM
	__asm
	{
		mov		eax, v;
		movss	xmm0, [eax];
		movss	xmm1, [eax + 4];
		movss	xmm2, [eax + 8];
		movaps	xmm3, xmm0;
		movaps	xmm4, xmm1;
		movaps	xmm5, xmm2;
		mulss	xmm0, xmm0;
		mulss	xmm1, xmm1;
		mulss	xmm2, xmm2;
		addss	xmm0, xmm1;
		addss	xmm0, xmm2;

		mov		ecx, pv;
		comiss	xmm0, _k_mt_float_zero;
		jae		pos_calc;

		xor		eax, eax;
		mov[ecx], eax;
		mov[ecx + 4], eax;
		mov[ecx + 8], eax;
		jmp		pos_end;

pos_calc:;
		rsqrtss	xmm0, xmm0;
		mulss	xmm3, xmm0;
		mulss	xmm4, xmm0;
		mulss	xmm5, xmm0;
		movss[ecx], xmm3;
		movss[ecx + 4], xmm4;
		movss[ecx + 8], xmm5;

pos_end:;
	}
#else
	float f=k_vec3_len(v);
	if (f>0.0f) f=1.0f/f;
	pv->x=v->x*f;
	pv->y=v->y*f;
	pv->z=v->z*f;
#endif
}

// 사원수 회전
void k_vec3_rot_quat(kVec3* pv, const kQuat* q)
{
	float x = q->x*q->x;
	float y = q->y*q->y;
	float z = q->z*q->z;
	float w = q->w*q->w;
	pv->x = k_atan2(2.0f*(q->y*q->z + q->x*q->w), -x - y + z + w);
	pv->y = k_asin(k_clamp(-2.0f*(q->x*q->z + q->y*q->w), -1.0f, 1.0f));
	pv->z = k_atan2(2.0f*(q->x * q->y + q->z*q->w), x - y - z + w);
}

// 행렬 회전
void k_vec3_rot_mat4(kVec3* pv, const kMat4* m)
{
	if (m->_31 == 0.0f && m->_33 == 0.0f)
	{
		if (m->_32 > 0.0f)
			pv->x = KM_PI_HALF + KM_PI;
		else
			pv->x = KM_PI_HALF;

		pv->z = k_atan2(m->_21, m->_23);
		pv->y = 0.0f;
	}
	else
	{
		pv->y = -k_atan2(m->_31, m->_33);
		pv->x = -k_atan2(m->_32, k_sqrt(m->_31*m->_31 + m->_33*m->_33));
		pv->z = k_atan2(m->_12, k_sqrt(m->_11*m->_11 + m->_13*m->_13));
	}
}

// 사원수+벡터 회전
void k_vec3_rot_quat_vec3(kVec3* pv, const kQuat* q, const kVec3* v)
{
	kQuat q1, q2;
	kVec3 t;

	q1.w = -k_vec3_dot((kVec3*)q, v);
	q1.x = v->x*q->w;
	q1.y = v->y*q->w;
	q1.z = v->z*q->w;

	k_vec3_cross(&t, (kVec3*)q, v);
	q1.x += t.x;
	q1.y += t.y;
	q1.z += t.z;

	q2.x = -q->x;
	q2.y = -q->y;
	q2.z = -q->z;
	q2.w = q->w;

	pv->x = q1.x*q2.w + q1.y*q2.z - q1.z*q2.y + q1.w*q2.x;
	pv->y = -q1.x*q2.z + q1.y*q2.w + q1.z*q2.x + q1.w*q2.y;
	pv->z = q1.x*q2.y - q1.y*q2.x + q1.z*q2.w + q1.w*q2.z;
}

// 근거리점
void k_vec3_cpt_line(kVec3* pv, const kLn3* l, const kVec3* v)
{
	kVec3 c, z;
	float d, t;

	k_vec3_sub(&c, v, &l->begin);
	k_vec3_sub(&z, &l->end, &l->begin);
	d = k_vec3_len(&z);
	k_vec3_mag(&z, &z, 1.0f / d);
	t = k_vec3_dot(&z, &c);

	if (t<0.0f)
		*pv = l->begin;
	else if (t>d)
		*pv = l->end;
	else
	{
		k_vec3_mag(&z, &z, t);
		k_vec3_add(pv, &l->begin, &z);
	}
}

// 합치기
void k_vec3_interpolate(kVec3* pv, const kVec3* left, const kVec3* right, float s)
{
	float f = 1.0f - s;
	pv->x = right->x*f + left->x*s;
	pv->y = right->y*f + left->y*s;
	pv->z = right->z*f + left->z*s;
}

// 바꾸기
void k_vec3_lerp(kVec3* pv, const kVec3* left, const kVec3* right, float s)
{
	pv->x = left->x + s*(right->x - left->x);
	pv->y = left->y + s*(right->y - left->y);
	pv->z = left->z + s*(right->z - left->z);
}

// 바꾸기+길이
void k_vec3_lerp_len(kVec3* pv, const kVec3* left, const kVec3* right, float s, float l)
{
	kVec3 v[2];
	k_vec3_norm(&v[0], left);
	k_vec3_norm(&v[1], right);
	pv->x = v[0].x + (v[1].x - v[0].x)*s;
	pv->y = v[0].y + (v[1].y - v[0].y)*s;
	pv->z = v[0].z + (v[1].z - v[0].z)*s;
	k_vec3_mag(pv, pv, l);
}

// 바릭 센트릭
void k_vec3_bary_centric(kVec3 *pv, const kVec3 *v1, const kVec3 *v2, const kVec3 *v3, float f, float g)
{
	pv->x = (1.0f - f - g)*(v1->x) + f*(v2->x) + g*(v3->x);
	pv->y = (1.0f - f - g)*(v1->y) + f*(v2->y) + g*(v3->y);
	pv->z = (1.0f - f - g)*(v1->z) + f*(v2->z) + g*(v3->z);
}

// 캐트멀롬
void k_vec3_catmull_rom(kVec3 *pv, const kVec3 *v0, const kVec3 *v1, const kVec3 *v2, const kVec3 *v3, float s)
{
	pv->x = 0.5f*(2.0f*v1->x + (v2->x - v0->x)*s + (2.0f*v0->x - 5.0f*v1->x + 4.0f*v2->x - v3->x)*s*s + (v3->x - 3.0f*v2->x + 3.0f*v1->x - v0->x)*s*s*s);
	pv->y = 0.5f*(2.0f*v1->y + (v2->y - v0->y)*s + (2.0f*v0->y - 5.0f*v1->y + 4.0f*v2->y - v3->y)*s*s + (v3->y - 3.0f*v2->y + 3.0f*v1->y - v0->y)*s*s*s);
	pv->z = 0.5f*(2.0f*v1->z + (v2->z - v0->z)*s + (2.0f*v0->z - 5.0f*v1->z + 4.0f*v2->z - v3->z)*s*s + (v3->z - 3.0f*v2->z + 3.0f*v1->z - v0->z)*s*s*s);
}

// 허밋
void k_vec3_hermite(kVec3 *pv, const kVec3 *v1, const kVec3 *t1, const kVec3 *v2, const kVec3 *t2, float s)
{
	float h1 = 2.0f*s*s*s - 3.0f*s*s + 1.0f;
	float h2 = s*s*s - 2.0f*s*s + s;
	float h3 = -2.0f*s*s*s + 3.0f*s*s;
	float h4 = s*s*s - s*s;
	pv->x = h1*v1->x + h2*t1->x + h3*v2->x + h4*t2->x;
	pv->y = h1*v1->y + h2*t1->y + h3*v2->y + h4*t2->y;
	pv->z = h1*v1->z + h2*t1->z + h3*v2->z + h4*t2->z;
}

// 폼 정규화
void k_vec3_form_norm(kVec3* pv, const kVec3* v0, const kVec3* v1, const kVec3* v2)
{
	kVec3 t[2];
	k_vec3_sub(&t[0], v1, v0);
	k_vec3_sub(&t[1], v2, v1);
	k_vec3_cross(pv, &t[0], &t[1]);
	k_vec3_mag(pv, pv, k_vec3_len(pv));
}

// 반사
kcham k_vec3_reflect(kVec3* pv, const kVec3* in, const kVec3* dir)
{
	kVec3 t;
	float dot, len;
	len = k_vec3_len(in);
	if (len == 0.0f)
		k_vec3_identify(&t);
	else
	{
		dot = 1.0f / len;
		k_vec3_mag(&t, in, dot);
	}
	dot = k_vec3_dot(&t, dir);
	if (dot >= 0.0f)
		return FALSE;
	else
	{
		k_vec3_set(pv, -2.0f*dot*dir->x + t.x, -2.0f*dot*dir->y + t.y, -2.0f*dot*dir->z + t.z);
		k_vec3_mag(pv, pv, len);
		return TRUE;
	}
}

// 요
float k_vec3_yaw(const kVec3* pv)
{
	return -k_atan(pv->z / pv->x) + ((pv->x > 0.0f) ? -KM_PI_HALF : KM_PI_HALF);
}

// 사이각
kcham k_vec3_between(const kVec3* p, const kVec3* b, const kVec3* e)
{
	kVec3 t;
	float f;
	k_vec3_sub(&t, e, b);
	f = k_vec3_len_sq(&t);
	return k_vec3_dist_sq(p, b) <= f && k_vec3_dist_sq(p, e) <= f;
}

// 선 충돌
kcham k_vec3_intersect_line(kVec3* pv, const kPln* pl, const kVec3* loc, const kVec3* dir)
{
	// v2->pl<-v1
	float dot = k_vec3_dot((const kVec3*)pl, dir);
	if (!dot)
	{
		k_vec3_identify(pv);
		return FALSE;
	}
	else
	{
		float tmp = (pl->d + k_vec3_dot((const kVec3*)pl, loc)) / dot;
		pv->x = loc->x - tmp*dir->x;
		pv->y = loc->y - tmp*dir->y;
		pv->z = loc->z - tmp*dir->z;
		return TRUE;
	}
}

// 점 충돌
kcham k_vec3_intersect_pt(kVec3* pv, const kPln* pl, const kVec3* v1, const kVec3* v2)
{
	kVec3 dir;
	k_vec3_sub(&dir, v2, v1);
	return k_vec3_intersect_line(pv, pl, v1, &dir);
}

// 점사이 충돌
kcham k_vec3_intersect_between_pt(kVec3* pv, const kPln* pl, const kVec3* v1, const kVec3* v2)
{
	kVec3 dir;
	k_vec3_sub(&dir, v2, v1);
	if (!k_vec3_intersect_line(pv, pl, v1, &dir))
		return FALSE;
	else
	{
		float f = k_vec3_len_sq(&dir);
		return k_vec3_dist_sq(pv, v1) <= f && k_vec3_dist_sq(pv, v2) <= f;
	}
}

// 면 충돌
kcham k_vec3_intersect_planes(kVec3* pv, const kPln* pl, const kPln* o1, const kPln* o2)
{
	kVec3 dir, loc;
	return (k_pln_test_intersect_pln(pl, &loc, &dir, o1)) ? k_vec3_intersect_line(pv, o2, &loc, &dir) : FALSE;
}


//////////////////////////////////////////////////////////////////////////
// 벡터4

// 단위 벡터
void k_vec4_norm(kVec4* pv, const kVec4* v)
{
#ifdef SB_MATH_ASM
	__asm
	{
		mov		eax, v;
		movss	xmm0, [eax];
		movss	xmm1, [eax + 4];
		movss	xmm2, [eax + 8];
		movss	xmm3, [eax + 12];
		movaps	xmm4, xmm0;
		movaps	xmm5, xmm1;
		movaps	xmm6, xmm2;
		movaps	xmm7, xmm3;
		mulss	xmm0, xmm0;
		mulss	xmm1, xmm1;
		mulss	xmm2, xmm2;
		mulss	xmm3, xmm3;
		addss	xmm0, xmm1;
		addss	xmm0, xmm2;
		addss	xmm0, xmm3;

		mov		ecx, pv;
		comiss	xmm0, _k_mt_float_zero;
		jae		pos_calc;

		xor		eax, eax;
		mov[ecx], eax;
		mov[ecx + 4], eax;
		mov[ecx + 8], eax;
		mov[ecx + 12], eax;
		jmp		pos_end;

pos_calc:;
		rsqrtss	xmm0, xmm0;
		mulss	xmm4, xmm0;
		mulss	xmm5, xmm0;
		mulss	xmm6, xmm0;
		mulss	xmm7, xmm0;
		movss[ecx], xmm4;
		movss[ecx + 4], xmm5;
		movss[ecx + 8], xmm6;
		movss[ecx + 12], xmm7;

pos_end:;
	}
#else
	float f=k_vec4_len(v);
	if (f>0.0f) f=1.0f/f;
	pv->x=v->x*f;
	pv->y=v->y*f;
	pv->z=v->z*f;
	pv->w=v->w*f;
#endif
}

// 외적
void k_vec4_cross(kVec4* pv, const kVec4* v1, const kVec4* v2, const kVec4* v3)
{
	k_vec4_set(pv,
		v1->y*(v2->z*v3->w - v3->z*v2->w) - v1->z*(v2->y*v3->w - v3->y*v2->w) + v1->w*(v2->y*v3->z - v2->z*v3->y),
		-(v1->x*(v2->z*v3->w - v3->z*v2->w) - v1->z*(v2->x*v3->w - v3->x*v2->w) + v1->w*(v2->x*v3->z - v3->x*v2->z)),
		v1->x*(v2->y*v3->w - v3->y*v2->w) - v1->y*(v2->x*v3->w - v3->x*v2->w) + v1->w*(v2->x*v3->y - v3->x*v2->y),
		-(v1->x*(v2->y*v3->z - v3->y*v2->z) - v1->y*(v2->x*v3->z - v3->x*v2->z) + v1->z*(v2->x*v3->y - v3->x*v2->y)));
}

// 합쳐
void k_vec4_interpolate(kVec4* pv, const kVec4* left, const kVec4* right, float s)
{
	float f = 1.0f - s;
	pv->x = right->x*f + left->x*s;
	pv->y = right->y*f + left->y*s;
	pv->z = right->z*f + left->z*s;
	pv->w = right->w*f + left->w*s;
}

// 바꿔
void k_vec4_lerp(kVec4* pv, const kVec4* left, const kVec4* right, float s)
{
	pv->x = left->x + s*(right->x - left->x);
	pv->y = left->y + s*(right->y - left->y);
	pv->z = left->z + s*(right->z - left->z);
	pv->w = left->w + s*(right->w - left->w);
}



//////////////////////////////////////////////////////////////////////////
// 사원수

// 외적
void k_quat_cross(kQuat* pq, const kQuat* q1, const kQuat* q2, const kQuat* q3)
{
	k_quat_set(pq,
		q1->y*(q2->z*q3->w - q3->z*q2->w) - q1->z*(q2->y*q3->w - q3->y*q2->w) + q1->w*(q2->y*q3->z - q2->z*q3->y),
		-(q1->x*(q2->z*q3->w - q3->z*q2->w) - q1->z*(q2->x*q3->w - q3->x*q2->w) + q1->w*(q2->x*q3->z - q3->x*q2->z)),
		q1->x*(q2->y*q3->w - q3->y*q2->w) - q1->y*(q2->x*q3->w - q3->x*q2->w) + q1->w*(q2->x*q3->y - q3->x*q2->y),
		-(q1->x*(q2->y*q3->z - q3->y*q2->z) - q1->y*(q2->x*q3->z - q3->x*q2->z) + q1->z*(q2->x*q3->y - q3->x*q2->y)));
}

// 단위
void k_quat_norm(kQuat* pq, const kQuat* q)
{
#ifdef SB_MATH_ASM
	__asm
	{
		mov		eax, q;
		movss	xmm0, [eax];
		movss	xmm1, [eax + 4];
		movss	xmm2, [eax + 8];
		movss	xmm3, [eax + 12];
		movaps	xmm4, xmm0;
		movaps	xmm5, xmm1;
		movaps	xmm6, xmm2;
		movaps	xmm7, xmm3;
		mulss	xmm0, xmm0;
		mulss	xmm1, xmm1;
		mulss	xmm2, xmm2;
		mulss	xmm3, xmm3;
		addss	xmm0, xmm1;
		addss	xmm0, xmm2;
		addss	xmm0, xmm3;

		mov		ecx, pq;
		comiss	xmm0, _k_mt_float_zero;
		jae		pos_calc;

		xor		eax, eax;
		mov[ecx], eax;
		mov[ecx + 4], eax;
		mov[ecx + 8], eax;
		mov[ecx + 12], eax;
		jmp		pos_end;

pos_calc:;
		rsqrtss	xmm0, xmm0;
		mulss	xmm4, xmm0;
		mulss	xmm5, xmm0;
		mulss	xmm6, xmm0;
		mulss	xmm7, xmm0;
		movss[ecx], xmm4;
		movss[ecx + 4], xmm5;
		movss[ecx + 8], xmm6;
		movss[ecx + 12], xmm7;

pos_end:;
	}
#else
	float f=k_quat_len(q);
	if (f>0.0f) f=1.0f/f;
	pq->x=q->x*f;
	pq->y=q->y*f;
	pq->z=q->z*f;
	pq->w=q->w*f;
#endif
}

// 곱셈
void k_quat_mul(kQuat* pq, const kQuat* left, const kQuat* right)
{
	k_quat_set(pq,
		left->x*right->w + left->y*right->z - left->z*right->y + left->w*right->x,
		-left->x*right->z + left->y*right->w + left->z*right->x + left->w*right->y,
		left->x*right->y - left->y*right->x + left->z*right->w + left->w*right->z,
		-left->x*right->x - left->y*right->y - left->z*right->z + left->w*right->w);
}

// 변환
void k_quat_slerp(kQuat* pq, const kQuat* left, const kQuat* right, float t)
{
	kQuat q1, q2;
	float f1, f2;
	float fc = k_quat_dot(left, right);

	if (fc < 0.0f)
	{
		k_quat_invert(&q1, left);
		q2 = *right;
		fc = -fc;
	}
	else
	{
		q1 = *left;
		q2 = *right;
	}

	if ((fc + 1.0f) > 0.05f)
	{
		if ((1.0f - fc) < 0.05f)
		{
			f1 = 1.0f - t;
			f2 = t;
		}
		else
		{
			float fl = k_acos(fc);
			float fr = k_sin(fl);

			f1 = k_sin(fl*(1.0f - t)) / fr;
			f2 = k_sin(fl*t) / fr;
		}
	}
	else
	{
		k_quat_set(&q2, -q1.y, q1.x, -q1.w, q1.z);
		f1 = k_sin(KM_PI*(0.5f - t));
		f2 = k_sin(KM_PI*t);
	}

	k_quat_set(pq, f1*q1.x + f2*q2.x, f1*q1.y + f2*q2.y, f1*q1.z + f2*q2.z, f1*q1.w + f2*q2.w);
}

// X회전
void k_quat_rot_x(kQuat* pq, float x)
{
	pq->y = pq->z = 0.0f;
	k_sincos(x*0.5f, &pq->x, &pq->w);
}

// Y회전
void k_quat_rot_y(kQuat* pq, float y)
{
	pq->x = pq->z = 0.0f;
	k_sincos(y*0.5f, &pq->y, &pq->w);
}

// Z회전
void k_quat_rot_z(kQuat* pq, float z)
{
	pq->x = pq->y = 0.0f;
	k_sincos(z*0.5f, &pq->z, &pq->w);
}

// 행렬 회전
void k_quat_rot_mat(kQuat* pq, const kMat4* m)
{
	float diag = m->_11 + m->_22 + m->_33 + 1.0f;
	float scale = 0.0f;
	float iscl = 0.0f;
	float norm;
	kQuat q;

	if (diag > 0.0f)
	{
		// 진단값에서 안전치
		scale = k_sqrt(diag) * 2.0f;
		iscl = 1.0f / scale;

		q.x = (m->_23 - m->_32) * iscl;
		q.y = (m->_31 - m->_13) * iscl;
		q.z = (m->_12 - m->_21) * iscl;
		q.w = 0.25f*scale;
	}
	else
	{
		// 필요한 스케일 만들기
		if (m->_11 > m->_22 && m->_11 > m->_33)
		{
			scale = k_sqrt(1.0f + m->_11 - m->_22 - m->_33) * 2.0f;
			iscl = 1.0f / scale;

			q.x = 0.25f*scale;
			q.y = (m->_12 + m->_21) * iscl;
			q.z = (m->_31 + m->_13) * iscl;
			q.w = (m->_23 - m->_32) * iscl;
		}
		else if (m->_22 > m->_33)
		{
			scale = k_sqrt(1.0f - m->_11 + m->_22 - m->_33) * 2.0f;
			iscl = 1.0f / scale;

			q.x = (m->_12 + m->_21) * iscl;
			q.y = 0.25f*scale;
			q.z = (m->_23 + m->_32) * iscl;
			q.w = (m->_31 - m->_13) * iscl;
		}
		else
		{
			scale = k_sqrt(1.0f - m->_11 - m->_22 + m->_33) * 2.0f;
			iscl = 1.0f / scale;

			q.x = (m->_31 + m->_13) * iscl;
			q.y = (m->_23 + m->_32) * iscl;
			q.z = 0.25f*scale;
			q.w = (m->_12 - m->_21) * iscl;
		}
	}

	norm = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w;
	if (norm == 1.0f)
		*pq = q;
	else
	{
		norm = 1.0f / k_sqrt(norm);
		pq->x = q.x*norm;
		pq->y = q.y*norm;
		pq->z = q.z*norm;
		pq->w = q.w*norm;
	}
}

// 벡터 회전
void k_quat_rot_vec(kQuat* pq, const kVec3* v)
{
	float angle;
	float rs, rc;
	float ys, yc;
	float ps, pc;
	float pcyc, psyc, pcys, psys;

	angle = v->x*0.5f;
	k_sincos(angle, &rs, &rc);

	angle = v->y*0.5f;
	k_sincos(angle, &ps, &pc);

	angle = v->z*0.5f;
	k_sincos(angle, &ys, &yc);

	pcyc = pc*yc;
	psyc = ps*yc;
	pcys = pc*ys;
	psys = ps*ys;

	pq->x = rs*pcyc - rc*psys;
	pq->y = rc*psyc + rs*pcys;
	pq->z = rc*pcys + rs*psyc;
	pq->w = rc*pcyc + rs*psys;
}

// 축 행렬 회전
void k_quat_rot_axis_vec(kQuat* pq, const kVec3* v, float angle)
{
	float s, c;
	k_sincos(angle*0.5f, &s, &c);
	k_quat_set(pq, v->x*s, v->y*s, v->z*s, c);
}

// 지수
void k_quat_exp(kQuat* pq, const kQuat* q)
{
	float n;
	float sn, cn;

	n = k_sqrt(q->x*q->x + q->y*q->y + q->z*q->z);
	if (n)
	{
		k_sincos(n, &sn, &cn);
		n = 1.0f / n;
		pq->x = sn*q->x*n;
		pq->y = sn*q->y*n;
		pq->z = sn*q->z*n;
		pq->w = cn;
	}
	else
	{
		pq->x = 0.0f;
		pq->y = 0.0f;
		pq->z = 0.0f;
		pq->w = 1.0f;
	}
}

// 로그
void k_quat_ln(kQuat* pq, const kQuat* q)
{
	float n, nv, t;

	n = k_quat_len_sq(q);
	if (n > 1.0001f)
	{
		pq->x = q->x;
		pq->y = q->y;
		pq->z = q->z;
		pq->w = 0.0f;
	}
	else if (n > 0.99999f)
	{
		nv = k_sqrt(q->x*q->x + q->y*q->y + q->z*q->z);
		t = k_atan2(nv, q->w) / nv;
		pq->x = t*q->x;
		pq->y = t*q->y;
		pq->z = t*q->z;
		pq->w = 0.0f;
	}
	else
	{
		// 법선이 1보다 작다. 이런일은 생기지 않는다!!!!
		k_trace(KLV_FATAL, "k.mathtype.quat", "normal is below '1'. what happen!!!!");
		pq->x = 0.0f;
		pq->y = 0.0f;
		pq->z = 0.0f;
		pq->w = 0.0f;
	}
}

// 역사원수
void k_quat_inv(kQuat* pq, const kQuat* q)
{
	kQuat t;
	float n;

	n = k_quat_len_sq(q);
	if (n == 0.0f)
	{
		pq->x = 0.0f;
		pq->y = 0.0f;
		pq->z = 0.0f;
		pq->w = 0.0f;
	}
	else
	{
		k_quat_conjugate(&t, q);
		n = 1.0f / n;
		pq->x = t.x*n;
		pq->y = t.y*n;
		pq->z = t.z*n;
		pq->w = t.w*n;
	}
}

// 합치기
void k_quat_interpolate(kQuat* pq, const kQuat* left, const kQuat* right, float s)
{
	float f = 1.0f - s;
	pq->x = right->x*f + left->x*s;
	pq->y = right->y*f + left->y*s;
	pq->z = right->z*f + left->z*s;
	pq->w = right->w*f + left->w*s;
}

// 바꿔
void k_quat_lerp(kQuat* pq, const kQuat* left, const kQuat* right, float s)
{
	pq->x = left->x + s*(right->x - left->x);
	pq->y = left->y + s*(right->y - left->y);
	pq->z = left->z + s*(right->z - left->z);
	pq->w = left->w + s*(right->w - left->w);
}

// 스쿼드
void k_quat_squad(kQuat* pq, const kQuat* q1, const kQuat* q2, const kQuat* q3, const kQuat* q4, float t)
{
	kQuat t1, t2;
	k_quat_slerp(&t1, q1, q4, t);
	k_quat_slerp(&t2, q2, q3, t);
	k_quat_slerp(pq, &t1, &t2, 2.0f*t*(1.0f - t));
}

// 바리 센트릭
void k_quat_bary_centric(kQuat* pq, const kQuat* q1, const kQuat* q2, const kQuat* q3, float f, float g)
{
	kQuat t1, t2;
	k_quat_slerp(&t1, q1, q2, f + g);
	k_quat_slerp(&t2, q1, q3, f + g);
	k_quat_slerp(pq, &t1, &t2, g / (f + g));
}


//////////////////////////////////////////////////////////////////////////
// 행렬

// 곱셈
void k_mat4_mul(kMat4* pm, const kMat4* left, const kMat4* right)
{
#ifdef SB_MATH_ASM
	__asm
	{
		mov			ecx, pm;
		mov			edx, left;
		mov			eax, right;

		movlps      xmm0, qword ptr[eax];
		movhps      xmm0, qword ptr[eax + 8];
		movlps      xmm1, qword ptr[eax + 10h];
		movhps      xmm1, qword ptr[eax + 18h];
		movlps      xmm2, qword ptr[eax + 20h];
		movhps      xmm2, qword ptr[eax + 28h];
		movlps      xmm3, qword ptr[eax + 30h];
		movhps      xmm3, qword ptr[eax + 38h];
		movss       xmm4, dword ptr[edx];
		movss       xmm5, dword ptr[edx + 4];
		movss       xmm6, dword ptr[edx + 8];
		movss       xmm7, dword ptr[edx + 0Ch];
		shufps      xmm4, xmm4, 0;
		shufps      xmm5, xmm5, 0;
		shufps      xmm6, xmm6, 0;
		shufps      xmm7, xmm7, 0;
		mulps       xmm4, xmm0;
		mulps       xmm5, xmm1;
		mulps       xmm6, xmm2;
		mulps       xmm7, xmm3;
		addps       xmm4, xmm5;
		addps       xmm6, xmm7;
		addps       xmm4, xmm6;
		movss       xmm5, dword ptr[edx + 10h];
		movss       xmm6, dword ptr[edx + 14h];
		movss       xmm7, dword ptr[edx + 18h];
		shufps      xmm5, xmm5, 0;
		shufps      xmm6, xmm6, 0;
		shufps      xmm7, xmm7, 0;
		mulps       xmm5, xmm0;
		mulps       xmm6, xmm1;
		mulps       xmm7, xmm2;
		addps       xmm5, xmm6;
		addps       xmm5, xmm7;
		movss       xmm6, dword ptr[edx + 1Ch];
		shufps      xmm6, xmm6, 0;
		mulps       xmm6, xmm3;
		addps       xmm5, xmm6;
		movss       xmm6, dword ptr[edx + 20h];
		movss       xmm7, dword ptr[edx + 24h];
		shufps      xmm6, xmm6, 0;
		shufps      xmm7, xmm7, 0;
		mulps       xmm6, xmm0;
		mulps       xmm7, xmm1;
		addps       xmm6, xmm7;
		movss       xmm7, dword ptr[edx + 28h];
		shufps      xmm7, xmm7, 0;
		mulps       xmm7, xmm2;
		addps       xmm6, xmm7;
		movss       xmm7, dword ptr[edx + 2Ch];
		shufps      xmm7, xmm7, 0;
		mulps       xmm7, xmm3;
		addps       xmm6, xmm7;
		movss       xmm7, dword ptr[edx + 30h];
		shufps      xmm7, xmm7, 0;
		mulps       xmm0, xmm7;
		movss       xmm7, dword ptr[edx + 34h];
		shufps      xmm7, xmm7, 0;
		mulps       xmm1, xmm7;
		movss       xmm7, dword ptr[edx + 38h];
		shufps      xmm7, xmm7, 0;
		mulps       xmm2, xmm7;
		movss       xmm7, dword ptr[edx + 3Ch];
		shufps      xmm7, xmm7, 0;
		mulps       xmm3, xmm7;
		movlps      qword ptr[ecx], xmm4;
		movhps      qword ptr[ecx + 8], xmm4;
		addps       xmm0, xmm1;
		movlps      qword ptr[ecx + 10h], xmm5;
		movhps      qword ptr[ecx + 18h], xmm5;
		addps       xmm2, xmm3;
		movlps      qword ptr[ecx + 20h], xmm6;
		movhps      qword ptr[ecx + 28h], xmm6;
		addps       xmm0, xmm2;
		movlps      qword ptr[ecx + 30h], xmm0;
		movhps      qword ptr[ecx + 38h], xmm0;
	}
#elif defined(SB_MM2)
	register __m128 r1, r2;
	register __m128 b1, b2, b3, b4;

	b1=_mm_loadu_ps(&right->_11);
	b2=_mm_loadu_ps(&right->_21);
	b3=_mm_loadu_ps(&right->_31);
	b4=_mm_loadu_ps(&right->_41);

	r1=_mm_mul_ps(_mm_set_ps1(left->_11),b1);
	r2=_mm_mul_ps(_mm_set_ps1(left->_21),b1);
	r1=_mm_add_ps(r1, _mm_mul_ps(_mm_set_ps1(left->_12),b2));
	r2=_mm_add_ps(r2, _mm_mul_ps(_mm_set_ps1(left->_22),b2));
	r1=_mm_add_ps(r1, _mm_mul_ps(_mm_set_ps1(left->_13),b3));
	r2=_mm_add_ps(r2, _mm_mul_ps(_mm_set_ps1(left->_23),b3));
	r1=_mm_add_ps(r1, _mm_mul_ps(_mm_set_ps1(left->_14),b4));
	r2=_mm_add_ps(r2, _mm_mul_ps(_mm_set_ps1(left->_24),b4));
	_mm_storeu_ps(&pm->_11, r1);
	_mm_storeu_ps(&pm->_21, r2);

	r1=_mm_mul_ps(_mm_set_ps1(left->_31),b1);
	r2=_mm_mul_ps(_mm_set_ps1(left->_41),b1);
	r1=_mm_add_ps(r1, _mm_mul_ps(_mm_set_ps1(left->_32),b2));
	r2=_mm_add_ps(r2, _mm_mul_ps(_mm_set_ps1(left->_42),b2));
	r1=_mm_add_ps(r1, _mm_mul_ps(_mm_set_ps1(left->_33),b3));
	r2=_mm_add_ps(r2, _mm_mul_ps(_mm_set_ps1(left->_43),b3));
	r1=_mm_add_ps(r1, _mm_mul_ps(_mm_set_ps1(left->_34),b4));
	r2=_mm_add_ps(r2, _mm_mul_ps(_mm_set_ps1(left->_44),b4));
	_mm_storeu_ps(&pm->_31, r1);
	_mm_storeu_ps(&pm->_41, r2);
#else
	kMat4 m;

	m._11 = left->_11*right->_11 + left->_12*right->_21 + left->_13*right->_31 + left->_14*right->_41;
	m._12 = left->_11*right->_12 + left->_12*right->_22 + left->_13*right->_32 + left->_14*right->_42;
	m._13 = left->_11*right->_13 + left->_12*right->_23 + left->_13*right->_33 + left->_14*right->_43;
	m._14 = left->_11*right->_14 + left->_12*right->_24 + left->_13*right->_34 + left->_14*right->_44;

	m._21 = left->_21*right->_11 + left->_22*right->_21 + left->_23*right->_31 + left->_24*right->_41;
	m._22 = left->_21*right->_12 + left->_22*right->_22 + left->_23*right->_32 + left->_24*right->_42;
	m._23 = left->_21*right->_13 + left->_22*right->_23 + left->_23*right->_33 + left->_24*right->_43;
	m._24 = left->_21*right->_14 + left->_22*right->_24 + left->_23*right->_34 + left->_24*right->_44;

	m._31 = left->_31*right->_11 + left->_32*right->_21 + left->_33*right->_31 + left->_34*right->_41;
	m._32 = left->_31*right->_12 + left->_32*right->_22 + left->_33*right->_32 + left->_34*right->_42;
	m._33 = left->_31*right->_13 + left->_32*right->_23 + left->_33*right->_33 + left->_34*right->_43;
	m._34 = left->_31*right->_14 + left->_32*right->_24 + left->_33*right->_34 + left->_34*right->_44;

	m._41 = left->_41*right->_11 + left->_42*right->_21 + left->_43*right->_31 + left->_44*right->_41;
	m._42 = left->_41*right->_12 + left->_42*right->_22 + left->_43*right->_32 + left->_44*right->_42;
	m._43 = left->_41*right->_13 + left->_42*right->_23 + left->_43*right->_33 + left->_44*right->_43;
	m._44 = left->_41*right->_14 + left->_42*right->_24 + left->_43*right->_34 + left->_44*right->_44;

	*pm=m;
#endif
}

// 전치
void k_mat4_tran(kMat4* pm, const kMat4* m)
{
#ifdef SB_MM2
	__m128 mm[4];

	mm[0] = _mm_unpacklo_ps(_mm_loadu_ps(&m->_11), _mm_loadu_ps(&m->_21));
	mm[1] = _mm_unpacklo_ps(_mm_loadu_ps(&m->_31), _mm_loadu_ps(&m->_41));
	mm[2] = _mm_unpackhi_ps(_mm_loadu_ps(&m->_11), _mm_loadu_ps(&m->_21));
	mm[3] = _mm_unpackhi_ps(_mm_loadu_ps(&m->_31), _mm_loadu_ps(&m->_41));

	_mm_storeu_ps(&pm->_11, _mm_movelh_ps(mm[0], mm[1]));
	_mm_storeu_ps(&pm->_21, _mm_movehl_ps(mm[1], mm[0]));
	_mm_storeu_ps(&pm->_31, _mm_movelh_ps(mm[2], mm[3]));
	_mm_storeu_ps(&pm->_41, _mm_movehl_ps(mm[3], mm[2]));
#else
	kMat4 t;
	t._11 = m->_11;
	t._12 = m->_21;
	t._13 = m->_31;
	t._14 = m->_41;
	t._21 = m->_12;
	t._22 = m->_22;
	t._23 = m->_32;
	t._24 = m->_42;
	t._31 = m->_13;
	t._32 = m->_23;
	t._33 = m->_33;
	t._34 = m->_43;
	t._41 = m->_14;
	t._42 = m->_24;
	t._43 = m->_34;
	t._44 = m->_44;
	*pm = t;
#endif
}

// 전치
void k_mat4_array_tran(kMat4* pm, kint cnt, kMat4* m)
{
#ifdef SB_MM2
	kint i;
	__m128 mm[4];

	for (i = 0; i < cnt; i++)
	{
		mm[0] = _mm_unpacklo_ps(_mm_loadu_ps(&m->_11), _mm_loadu_ps(&m->_21));
		mm[1] = _mm_unpacklo_ps(_mm_loadu_ps(&m->_31), _mm_loadu_ps(&m->_41));
		mm[2] = _mm_unpackhi_ps(_mm_loadu_ps(&m->_11), _mm_loadu_ps(&m->_21));
		mm[3] = _mm_unpackhi_ps(_mm_loadu_ps(&m->_31), _mm_loadu_ps(&m->_41));

		_mm_storeu_ps(&pm->_11, _mm_movelh_ps(mm[0], mm[1]));
		_mm_storeu_ps(&pm->_21, _mm_movehl_ps(mm[1], mm[0]));
		_mm_storeu_ps(&pm->_31, _mm_movelh_ps(mm[2], mm[3]));
		_mm_storeu_ps(&pm->_41, _mm_movehl_ps(mm[3], mm[2]));

		m++; pm++;
	}
#else
	kint i;
	for (i = 0; i < cnt; i++)
		k_mat4_tran(&pm[i], &m[i]);
#endif
}

// 역행렬
void k_mat4_inv(kMat4* pm, float *retdet, const kMat4* m)
{
#ifdef SB_MM2
	__m128 A, B, C, D;
	__m128 iA, iB, iC, iD;
	__m128 DC, AB;
	__m128 dA, dB, dC, dD;
	__m128 det, d, d1, d2;
	__m128 rd;
	__m128 b1, b2, b3, b4;

	b1 = _mm_loadu_ps(&m->_11);
	b2 = _mm_loadu_ps(&m->_21);
	b3 = _mm_loadu_ps(&m->_31);
	b4 = _mm_loadu_ps(&m->_41);

	A = _mm_movelh_ps(b1, b2);
	B = _mm_movehl_ps(b2, b1);
	C = _mm_movelh_ps(b3, b4);
	D = _mm_movehl_ps(b4, b3);

	AB = _mm_mul_ps(_mm_shuffle_ps(A, A, 0x0F), B);
	AB = _mm_sub_ps(AB, _mm_mul_ps(_mm_shuffle_ps(A, A, 0xA5), _mm_shuffle_ps(B, B, 0x4E)));
	DC = _mm_mul_ps(_mm_shuffle_ps(D, D, 0x0F), C);
	DC = _mm_sub_ps(DC, _mm_mul_ps(_mm_shuffle_ps(D, D, 0xA5), _mm_shuffle_ps(C, C, 0x4E)));

	dA = _mm_mul_ps(_mm_shuffle_ps(A, A, 0x5F), A);
	dA = _mm_sub_ss(dA, _mm_movehl_ps(dA, dA));
	dB = _mm_mul_ps(_mm_shuffle_ps(B, B, 0x5F), B);
	dB = _mm_sub_ss(dB, _mm_movehl_ps(dB, dB));

	dC = _mm_mul_ps(_mm_shuffle_ps(C, C, 0x5F), C);
	dC = _mm_sub_ss(dC, _mm_movehl_ps(dC, dC));
	dD = _mm_mul_ps(_mm_shuffle_ps(D, D, 0x5F), D);
	dD = _mm_sub_ss(dD, _mm_movehl_ps(dD, dD));

	d = _mm_mul_ps(_mm_shuffle_ps(DC, DC, 0xD8), AB);

	iD = _mm_mul_ps(_mm_shuffle_ps(C, C, 0xA0), _mm_movelh_ps(AB, AB));
	iD = _mm_add_ps(iD, _mm_mul_ps(_mm_shuffle_ps(C, C, 0xF5), _mm_movehl_ps(AB, AB)));
	iA = _mm_mul_ps(_mm_shuffle_ps(B, B, 0xA0), _mm_movelh_ps(DC, DC));
	iA = _mm_add_ps(iA, _mm_mul_ps(_mm_shuffle_ps(B, B, 0xF5), _mm_movehl_ps(DC, DC)));

	d = _mm_add_ps(d, _mm_movehl_ps(d, d));
	d = _mm_add_ss(d, _mm_shuffle_ps(d, d, 1));
	d1 = _mm_mul_ps(dA, dD);
	d2 = _mm_mul_ps(dB, dC);

	iD = _mm_sub_ps(_mm_mul_ps(D, _mm_shuffle_ps(dA, dA, 0)), iD);
	iA = _mm_sub_ps(_mm_mul_ps(A, _mm_shuffle_ps(dD, dD, 0)), iA);

	det = _mm_sub_ss(_mm_add_ss(d1, d2), d);
	rd = _mm_div_ss(_mm_set_ss(1.0f), det);
	// ZERO_SINGULAR
	//rd=_mm_and_ps(_mm_cmpneq_ss(det,_mm_setzero_ps()),rd);

	iB = _mm_mul_ps(D, _mm_shuffle_ps(AB, AB, 0x33));
	iB = _mm_sub_ps(iB, _mm_mul_ps(_mm_shuffle_ps(D, D, 0xB1), _mm_shuffle_ps(AB, AB, 0x66)));
	iC = _mm_mul_ps(A, _mm_shuffle_ps(DC, DC, 0x33));
	iC = _mm_sub_ps(iC, _mm_mul_ps(_mm_shuffle_ps(A, A, 0xB1), _mm_shuffle_ps(DC, DC, 0x66)));

	rd = _mm_shuffle_ps(rd, rd, 0);
	rd = _mm_xor_ps(rd, _K_MM_PNNP);

	iB = _mm_sub_ps(_mm_mul_ps(C, _mm_shuffle_ps(dB, dB, 0)), iB);
	iC = _mm_sub_ps(_mm_mul_ps(B, _mm_shuffle_ps(dC, dC, 0)), iC);

	iA = _mm_div_ps(iA, rd);
	iB = _mm_div_ps(iB, rd);
	iC = _mm_div_ps(iC, rd);
	iD = _mm_div_ps(iD, rd);

	_mm_storeu_ps(&pm->_11, _mm_shuffle_ps(iA, iB, 0x77));
	_mm_storeu_ps(&pm->_21, _mm_shuffle_ps(iA, iB, 0x22));
	_mm_storeu_ps(&pm->_31, _mm_shuffle_ps(iC, iD, 0x77));
	_mm_storeu_ps(&pm->_41, _mm_shuffle_ps(iC, iD, 0x22));

	if (retdet)
		*retdet = *(float*)&det;
#else
	kMat4	t;
	float		f;

	f = (m->_11 * m->_22 - m->_21 * m->_12) * (m->_33 * m->_44 - m->_43 * m->_34) - (m->_11 * m->_32 - m->_31 * m->_12) * (m->_23 * m->_44 - m->_43 * m->_24)
		+ (m->_11 * m->_42 - m->_41 * m->_12) * (m->_23 * m->_34 - m->_33 * m->_24) + (m->_21 * m->_32 - m->_31 * m->_22) * (m->_13 * m->_44 - m->_43 * m->_14)
		- (m->_21 * m->_42 - m->_41 * m->_22) * (m->_13 * m->_34 - m->_33 * m->_14) + (m->_31 * m->_42 - m->_41 * m->_32) * (m->_13 * m->_24 - m->_23 * m->_14);

	if (f == 0.0f)
	{
		if (retdet)
			*retdet = 0.0f;
		if (pm != m)
			*pm = *m;
		return;
	}
	f = 1.0f / f;

	t._11 = f * (m->_22 * (m->_33 * m->_44 - m->_43 * m->_34) + m->_32 * (m->_43 * m->_24 - m->_23 * m->_44) + m->_42 * (m->_23 * m->_34 - m->_33 * m->_24));
	t._21 = f * (m->_23 * (m->_31 * m->_44 - m->_41 * m->_34) + m->_33 * (m->_41 * m->_24 - m->_21 * m->_44) + m->_43 * (m->_21 * m->_34 - m->_31 * m->_24));
	t._31 = f * (m->_24 * (m->_31 * m->_42 - m->_41 * m->_32) + m->_34 * (m->_41 * m->_22 - m->_21 * m->_42) + m->_44 * (m->_21 * m->_32 - m->_31 * m->_22));
	t._41 = f * (m->_21 * (m->_42 * m->_33 - m->_32 * m->_43) + m->_31 * (m->_22 * m->_43 - m->_42 * m->_23) + m->_41 * (m->_32 * m->_23 - m->_22 * m->_33));
	t._12 = f * (m->_32 * (m->_13 * m->_44 - m->_43 * m->_14) + m->_42 * (m->_33 * m->_14 - m->_13 * m->_34) + m->_12 * (m->_43 * m->_34 - m->_33 * m->_44));
	t._22 = f * (m->_33 * (m->_11 * m->_44 - m->_41 * m->_14) + m->_43 * (m->_31 * m->_14 - m->_11 * m->_34) + m->_13 * (m->_41 * m->_34 - m->_31 * m->_44));
	t._32 = f * (m->_34 * (m->_11 * m->_42 - m->_41 * m->_12) + m->_44 * (m->_31 * m->_12 - m->_11 * m->_32) + m->_14 * (m->_41 * m->_32 - m->_31 * m->_42));
	t._42 = f * (m->_31 * (m->_42 * m->_13 - m->_12 * m->_43) + m->_41 * (m->_12 * m->_33 - m->_32 * m->_13) + m->_11 * (m->_32 * m->_43 - m->_42 * m->_33));
	t._13 = f * (m->_42 * (m->_13 * m->_24 - m->_23 * m->_14) + m->_12 * (m->_23 * m->_44 - m->_43 * m->_24) + m->_22 * (m->_43 * m->_14 - m->_13 * m->_44));
	t._23 = f * (m->_43 * (m->_11 * m->_24 - m->_21 * m->_14) + m->_13 * (m->_21 * m->_44 - m->_41 * m->_24) + m->_23 * (m->_41 * m->_14 - m->_11 * m->_44));
	t._33 = f * (m->_44 * (m->_11 * m->_22 - m->_21 * m->_12) + m->_14 * (m->_21 * m->_42 - m->_41 * m->_22) + m->_24 * (m->_41 * m->_12 - m->_11 * m->_42));
	t._43 = f * (m->_41 * (m->_22 * m->_13 - m->_12 * m->_23) + m->_11 * (m->_42 * m->_23 - m->_22 * m->_43) + m->_21 * (m->_12 * m->_43 - m->_42 * m->_13));
	t._14 = f * (m->_12 * (m->_33 * m->_24 - m->_23 * m->_34) + m->_22 * (m->_13 * m->_34 - m->_33 * m->_14) + m->_32 * (m->_23 * m->_14 - m->_13 * m->_24));
	t._24 = f * (m->_13 * (m->_31 * m->_24 - m->_21 * m->_34) + m->_23 * (m->_11 * m->_34 - m->_31 * m->_14) + m->_33 * (m->_21 * m->_14 - m->_11 * m->_24));
	t._34 = f * (m->_14 * (m->_31 * m->_22 - m->_21 * m->_32) + m->_24 * (m->_11 * m->_32 - m->_31 * m->_12) + m->_34 * (m->_21 * m->_12 - m->_11 * m->_22));
	t._44 = f * (m->_11 * (m->_22 * m->_33 - m->_32 * m->_23) + m->_21 * (m->_32 * m->_13 - m->_12 * m->_33) + m->_31 * (m->_12 * m->_23 - m->_22 * m->_13));

	if (retdet)
		*retdet = f;
	*pm = t;
#endif
}

// 벡터 회전
void k_mat4_rot_vec(kMat4* pm, const kVec3* v)
{
	float sr, sp, sy;
	float cr, cp, cy;
	float srsp, crsp;

	k_sincos(v->x, &sr, &cr);
	k_sincos(v->y, &sp, &cp);
	k_sincos(v->z, &sy, &cy);
	srsp = sr*sp;
	crsp = cr*sp;

	pm->_11 = cp*cy;
	pm->_12 = cp*sy;
	pm->_13 = -sp;
	pm->_14 = 0.0f;

	pm->_21 = srsp*cy - cr*sy;
	pm->_22 = srsp*sy + cr*cy;
	pm->_23 = sr*cp;
	pm->_24 = 0.0f;

	pm->_31 = crsp*cy + sr*sy;
	pm->_32 = crsp*sy - sr*cy;
	pm->_33 = cr*cp;
	pm->_34 = 0.0f;

	pm->_41 = 0.0f;
	pm->_42 = 0.0f;
	pm->_43 = 0.0f;
	pm->_44 = 1.0f;
}

// 사원수 회전
void k_mat4_rot_quat(kMat4* pm, const kQuat* q)
{
	pm->_11 = 1.0f - 2.0f*q->y*q->y - 2.0f*q->z*q->z;
	pm->_12 = 2.0f*q->x*q->y + 2.0f*q->z*q->w;
	pm->_13 = 2.0f*q->x*q->z - 2.0f*q->y*q->w;
	pm->_14 = 0.0f;

	pm->_21 = 2.0f*q->x*q->y - 2.0f*q->z*q->w;
	pm->_22 = 1.0f - 2.0f*q->x*q->x - 2.0f*q->z*q->z;
	pm->_23 = 2.0f*q->z*q->y + 2.0f*q->x*q->w;
	pm->_24 = 0.0f;

	pm->_31 = 2.0f*q->x*q->z + 2.0f*q->y*q->w;
	pm->_32 = 2.0f*q->z*q->y - 2.0f*q->x*q->w;
	pm->_33 = 1.0f - 2.0f*q->x*q->x - 2.0f*q->y*q->y;
	pm->_34 = 0.0f;

	pm->_41 = 0.0f;
	pm->_42 = 0.0f;
	pm->_43 = 0.0f;
	pm->_44 = 1.0f;
}

// X회전
void k_mat4_rot_x(kMat4* pm, float f)
{
#ifdef SB_MATH_ASM
	__asm
	{
		xorps	xmm0, xmm0;
		mov		eax, pm;
		fld		float ptr f;
		movlps[eax + 0x10], xmm0;
		movhps[eax + 0x18], xmm0;
		fsincos;
		fst		float ptr[eax + 0x14];
		movlps[eax + 0x20], xmm0;
		movhps[eax + 0x28], xmm0;
		fstp	float ptr[eax + 0x28];
		fst		float ptr[eax + 0x18];
		fchs;
		movlps[eax], xmm0;
		movhps[eax + 0x08], xmm0;
		fstp	float ptr[eax + 0x24];
		fld1;
		fst		float ptr[eax + 0x00];
		movlps[eax + 0x30], xmm0;
		movhps[eax + 0x38], xmm0;
		fstp	float ptr[eax + 0x3C];
	}
#else
	float fs, fc;

	k_sincos(f, &fs, &fc);

	pm->_11 = 1.0f; pm->_12 =  0.0f; pm->_13 = 0.0f; pm->_14 = 0.0f;
	pm->_21 = 0.0f; pm->_22 =  fc;   pm->_23 = fs;   pm->_24 = 0.0f;
	pm->_31 = 0.0f; pm->_32 = -fs;   pm->_33 = fc;   pm->_34 = 0.0f;
	pm->_41 = 0.0f; pm->_42 =  0.0f; pm->_43 = 0.0f; pm->_44 = 1.0f;
#endif
}

// Y회전
void k_mat4_rot_y(kMat4* pm, float f)
{
#ifdef SB_MATH_ASM
	__asm
	{
		xorps	xmm0, xmm0;
		mov		eax, pm;
		fld		float ptr f;
		movlps[eax], xmm0;
		movhps[eax + 0x08], xmm0;
		fsincos;
		fst     float ptr[eax + 0x00];
		movlps[eax + 0x20], xmm0;
		movhps[eax + 0x28], xmm0;
		fstp    float ptr[eax + 0x28];
		fst     float ptr[eax + 0x20];
		fchs;
		movlps[eax + 0x10], xmm0;
		movhps[eax + 0x18], xmm0;
		fstp    float ptr[eax + 0x08];
		fld1;
		fst     float ptr[eax + 0x14];
		movlps[eax + 0x30], xmm0;
		movhps[eax + 0x38], xmm0;
		fstp    float ptr[eax + 0x3C];
	}
#else
	float fs, fc;

	k_sincos(f, &fs, &fc);

	pm->_11 = fc;   pm->_12 =  0.0f; pm->_13 = -fs;  pm->_14 = 0.0f;
	pm->_21 = 0.0f; pm->_22 =  1.0f; pm->_23 = 0.0f; pm->_24 = 0.0f;
	pm->_31 = fs;   pm->_32 =  0.0f; pm->_33 = fc;   pm->_34 = 0.0f;
	pm->_41 = 0.0f; pm->_42 =  0.0f; pm->_43 = 0.0f; pm->_44 = 1.0f;
#endif
}

// Z회전
void k_mat4_rot_z(kMat4* pm, float f)
{
#ifdef SB_MATH_ASM
	__asm
	{
		xorps   xmm0, xmm0;
		mov     eax, pm;
		fld     float ptr f;
		movlps[eax], xmm0;
		movhps[eax + 0x08], xmm0;
		fsincos;
		fst     float ptr[eax + 0x00];
		movlps[eax + 0x10], xmm0;
		movhps[eax + 0x18], xmm0;
		fstp    float ptr[eax + 0x14];
		fst     float ptr[eax + 0x04];
		fchs;
		movlps[eax + 0x20], xmm0;
		movhps[eax + 0x28], xmm0;
		fstp    float ptr[eax + 0x10];
		fld1;
		fst     float ptr[eax + 0x28];
		movlps[eax + 0x30], xmm0;
		movhps[eax + 0x38], xmm0;
		fstp    float ptr[eax + 0x3C];
	}
#else
	float fs, fc;

	k_sincos(f, &fs, &fc);

	pm->_11 = fc;   pm->_12 =  fs;   pm->_13 = 0.0f; pm->_14 = 0.0f;
	pm->_21 = -fs;  pm->_22 =  fc;   pm->_23 = 0.0f; pm->_24 = 0.0f;
	pm->_31 = 0.0f; pm->_32 =  0.0f; pm->_33 = 1.0f; pm->_34 = 0.0f;
	pm->_41 = 0.0f; pm->_42 =  0.0f; pm->_43 = 0.0f; pm->_44 = 1.0f;
#endif
}

// 전치곱
void k_mat4_mul_tran(kMat4* pm, const kMat4* left, const kMat4* right)
{
	kMat4 m;
	k_mat4_mul(&m, left, right);
	k_mat4_tran(pm, &m);
}

// 행렬식
float k_mat4_det(const kMat4* m)
{
#ifdef SB_MM2
	__m128 va, vb, vc;
	__m128 r1, r2, r3;
	__m128 t1, t2, sum;
	__m128 det;

	t1 = _mm_loadu_ps(&m->_41);
	t2 = _mm_loadu_ps(&m->_31);
	t2 = _mm_ror_ps(t2, 1);
	vc = _mm_mul_ps(t2, _mm_ror_ps(t1, 0));
	va = _mm_mul_ps(t2, _mm_ror_ps(t1, 2));
	vb = _mm_mul_ps(t2, _mm_ror_ps(t1, 3));

	r1 = _mm_sub_ps(_mm_ror_ps(va, 1), _mm_ror_ps(vc, 2));
	r2 = _mm_sub_ps(_mm_ror_ps(vb, 2), _mm_ror_ps(vb, 0));
	r3 = _mm_sub_ps(_mm_ror_ps(va, 0), _mm_ror_ps(vc, 1));

	va = _mm_loadu_ps(&m->_21);
	va = _mm_ror_ps(va, 1);	sum = _mm_mul_ps(va, r1);
	vb = _mm_ror_ps(va, 1);	sum = _mm_add_ps(sum, _mm_mul_ps(vb, r2));
	vc = _mm_ror_ps(vb, 1);	sum = _mm_add_ps(sum, _mm_mul_ps(vc, r3));

	det = _mm_mul_ps(sum, _mm_loadu_ps(&m->_11));
	det = _mm_add_ps(det, _mm_movehl_ps(det, det));
	det = _mm_sub_ss(det, _mm_shuffle_ps(det, det, 1));

	return *(((float*)&det) + 0);
#else
	float		f;

	f = (m->_11 * m->_22 - m->_21 * m->_12) * (m->_33 * m->_44 - m->_43 * m->_34) - (m->_11 * m->_32 - m->_31 * m->_12) * (m->_23 * m->_44 - m->_43 * m->_24)
		+ (m->_11 * m->_42 - m->_41 * m->_12) * (m->_23 * m->_34 - m->_33 * m->_24) + (m->_21 * m->_32 - m->_31 * m->_22) * (m->_13 * m->_44 - m->_43 * m->_14)
		- (m->_21 * m->_42 - m->_41 * m->_22) * (m->_13 * m->_34 - m->_33 * m->_14) + (m->_31 * m->_42 - m->_41 * m->_32) * (m->_13 * m->_24 - m->_23 * m->_14);

	return f == 0.0f ? 0.0f : 1.0f / f;
#endif
}

// 대체, 바꿔+길이
void k_mat4_rep_lerp_len(kMat4* pm, const kVec3* left, const kVec3* right, float s)
{
	pm->_11 = left->x + s*(right->x - left->x);
	pm->_22 = left->y + s*(right->y - left->y);
	pm->_33 = left->z + s*(right->z - left->z);
}

// 대체, 사원수 회전
void k_mat4_rep_rot_quat(kMat4* pm, const kQuat* q)
{
	pm->_11 = 1.0f - 2.0f*q->y*q->y - 2.0f*q->z*q->z;
	pm->_12 = 2.0f*q->x*q->y + 2.0f*q->z*q->w;
	pm->_13 = 2.0f*q->x*q->z - 2.0f*q->y*q->w;
	pm->_21 = 2.0f*q->x*q->y - 2.0f*q->z*q->w;
	pm->_22 = 1.0f - 2.0f*q->x*q->x - 2.0f*q->z*q->z;
	pm->_23 = 2.0f*q->z*q->y + 2.0f*q->x*q->w;
	pm->_31 = 2.0f*q->x*q->z + 2.0f*q->y*q->w;
	pm->_32 = 2.0f*q->z*q->y - 2.0f*q->x*q->w;
	pm->_33 = 1.0f - 2.0f*q->x*q->x - 2.0f*q->y*q->y;
}

// 루깻, 왼손
void k_mat4_lookat_lh(kMat4* pm, const kVec3* Eye, const kVec3* At, const kVec3* Up)
{
	kVec3 vx, vy, vz;
	k_vec3_sub(&vz, At, Eye);		k_vec3_norm(&vz, &vz);
	k_vec3_cross(&vx, Up, &vz);	k_vec3_norm(&vx, &vx);
	k_vec3_cross(&vy, &vz, &vx);
	pm->_11 = vx.x;	pm->_12 = vy.x;	pm->_13 = vz.x;	pm->_14 = 0.0f;
	pm->_21 = vx.y;	pm->_22 = vy.y;	pm->_23 = vz.y;	pm->_24 = 0.0f;
	pm->_31 = vx.z;	pm->_32 = vy.z;	pm->_33 = vz.z;	pm->_34 = 0.0f;
	pm->_41 = -k_vec3_dot(&vx, Eye);
	pm->_42 = -k_vec3_dot(&vy, Eye);
	pm->_43 = -k_vec3_dot(&vz, Eye);
	pm->_44 = 1.0f;
}

// 루깻, 오른손
void k_mat4_lookat_rh(kMat4* pm, const kVec3* Eye, const kVec3* At, const kVec3* Up)
{
	kVec3 vx, vy, vz;
	k_vec3_sub(&vz, Eye, At);		k_vec3_norm(&vz, &vz);
	k_vec3_cross(&vx, Up, &vz);	k_vec3_norm(&vx, &vx);
	k_vec3_cross(&vy, &vz, &vx);
	pm->_11 = vx.x;	pm->_12 = vy.x;	pm->_13 = vz.x;	pm->_14 = 0.0f;
	pm->_21 = vx.y;	pm->_22 = vy.y;	pm->_23 = vz.y;	pm->_24 = 0.0f;
	pm->_31 = vx.z;	pm->_32 = vy.z;	pm->_33 = vz.z;	pm->_34 = 0.0f;
	pm->_41 = -k_vec3_dot(&vx, Eye);
	pm->_42 = -k_vec3_dot(&vy, Eye);
	pm->_43 = -k_vec3_dot(&vz, Eye);
	pm->_44 = 1.0f;
}

// 퍼스, 왼손
void k_mat4_perspective_lh(kMat4* pm, float fov, float aspect, float zn, float zf)
{
	float f = 1.0f / k_tan(fov*0.5f);
	float q = zf / (zf - zn);
	pm->_11 = f / aspect;		pm->_12 = 0.0f;			pm->_13 = 0.0f;			pm->_14 = 0.0f;
	pm->_21 = 0.0f;			pm->_22 = f;				pm->_23 = 0.0f;			pm->_24 = 0.0f;
	pm->_31 = 0.0f;			pm->_32 = 0.0f;			pm->_33 = q;				pm->_34 = 1.0f;
	pm->_41 = 0.0f;			pm->_42 = 0.0f;			pm->_43 = -zn*q;			pm->_44 = 0.0f;
}

// 퍼스, 오른손
void k_mat4_perspective_rh(kMat4* pm, float fov, float aspect, float zn, float zf)
{
	float f = 1.0f / k_tan(fov*0.5f);
	float q = zf / (zn - zf);
	pm->_11 = f / aspect;		pm->_12 = 0.0f;			pm->_13 = 0.0f;			pm->_14 = 0.0f;
	pm->_21 = 0.0f;			pm->_22 = f;				pm->_23 = 0.0f;			pm->_24 = 0.0f;
	pm->_31 = 0.0f;			pm->_32 = 0.0f;			pm->_33 = q;				pm->_34 = -1.0f;
	pm->_41 = 0.0f;			pm->_42 = 0.0f;			pm->_43 = zn*q;			pm->_44 = 0.0f;
}

// 오소, 왼손
void k_mat4_ortho_lh(kMat4* pm, float width, float height, float zn, float zf)
{
	pm->_11 = 2.0f / width;		pm->_12 = 0.0f;			pm->_13 = 0.0f;			pm->_14 = 0.0f;
	pm->_21 = 0.0f;			pm->_22 = 2.0f / height;	pm->_23 = 0.0f;			pm->_24 = 0.0f;
	pm->_31 = 0.0f;			pm->_32 = 0.0f;			pm->_33 = 1.0f / (zf - zn);	pm->_34 = 0.0f;
	pm->_41 = 0.0f;			pm->_42 = 0.0f;			pm->_43 = zn / (zn - zf);		pm->_44 = 1.0f;
}

// 오소, 오른손
void k_mat4_ortho_rh(kMat4* pm, float width, float height, float zn, float zf)
{
	pm->_11 = 2.0f / width;		pm->_12 = 0.0f;			pm->_13 = 0.0f;			pm->_14 = 0.0f;
	pm->_21 = 0.0f;			pm->_22 = 2.0f / height;	pm->_23 = 0.0f;			pm->_24 = 0.0f;
	pm->_31 = 0.0f;			pm->_32 = 0.0f;			pm->_33 = 1.0f / (zn - zf);	pm->_34 = 0.0f;
	pm->_41 = 0.0f;			pm->_42 = 0.0f;			pm->_43 = zn / (zn - zf);		pm->_44 = 1.0f;
}

// 오소, 오프, 왼손
void k_mat4_ortho_offcenter_lh(kMat4* pm, float l, float r, float b, float t, float zn, float zf)
{
	pm->_11 = 2.0f / (r - l);		pm->_12 = 0.0f;			pm->_13 = 0.0f;			pm->_14 = 0.0f;
	pm->_21 = 0.0f;			pm->_22 = 2.0f / (t - b);		pm->_23 = 0.0f;			pm->_24 = 0.0f;
	pm->_31 = 0.0f;			pm->_32 = 0.0f;			pm->_33 = 1.0f / (zf - zn);	pm->_34 = 0.0f;
	pm->_41 = (l + r) / (l - r);	pm->_42 = (t + b) / (b - t);	pm->_43 = zn / (zn - zf);		pm->_44 = 1.0f;
}

// 오소, 오프, 오른손
void k_mat4_ortho_offcenter_rh(kMat4* pm, float l, float r, float b, float t, float zn, float zf)
{
	pm->_11 = 2.0f / (r - l);		pm->_12 = 0.0f;			pm->_13 = 0.0f;			pm->_14 = 0.0f;
	pm->_21 = 0.0f;			pm->_22 = 2.0f / (t - b);		pm->_23 = 0.0f;			pm->_24 = 0.0f;
	pm->_31 = 0.0f;			pm->_32 = 0.0f;			pm->_33 = 1.0f / (zn - zf);	pm->_34 = 0.0f;
	pm->_41 = (l + r) / (l - r);	pm->_42 = (t + b) / (b - t);	pm->_43 = zn / (zn - zf);		pm->_44 = 1.0f;
}

// 뷰포트
void k_mat4_viewport(kMat4* pm, float x, float y, float width, float height)
{
	pm->_11 = width*0.5f;		pm->_12 = 0.0f;			pm->_13 = 0.0f;			pm->_14 = 0.0f;
	pm->_21 = 0.0f;			pm->_22 = height*-0.5f;	pm->_23 = 0.0f;			pm->_24 = 0.0f;
	pm->_31 = 0.0f;			pm->_32 = 0.0f;			pm->_33 = 1.0f;			pm->_34 = 0.0f;
	pm->_41 = x + pm->_11;		pm->_42 = y - pm->_22;		pm->_43 = 0.0f;			pm->_44 = 1.0f;
}

// 그림자
void k_mat4_shadow(kMat4* pm, const kVec4* lgt, const kPln* pl)
{
	float d = pl->a*lgt->x + pl->b*lgt->y + pl->c*lgt->z + pl->d;
	if (lgt->w == 0.0f)
	{
		pm->_11 = -pl->a*lgt->x + d;	pm->_12 = -pl->a*lgt->y;		pm->_13 = -pl->a*lgt->z;		pm->_14 = 0.0f;
		pm->_21 = -pl->b*lgt->x;		pm->_22 = -pl->b*lgt->y + d;	pm->_23 = -pl->b*lgt->z;		pm->_24 = 0.0f;
		pm->_31 = -pl->c*lgt->x;		pm->_32 = -pl->c*lgt->y;		pm->_33 = -pl->c*lgt->z + d;	pm->_34 = 0.0f;
		pm->_41 = -pl->d*lgt->x;		pm->_42 = -pl->d*lgt->y;		pm->_43 = -pl->d*lgt->z;		pm->_44 = d;
	}
	else
	{
		pm->_11 = -pl->a*lgt->x + d;	pm->_12 = -pl->a*lgt->y;		pm->_13 = -pl->a*lgt->z;		pm->_14 = -pl->a*lgt->w;
		pm->_21 = -pl->b*lgt->x;		pm->_22 = -pl->b*lgt->y + d;	pm->_23 = -pl->b*lgt->z;		pm->_24 = -pl->b*lgt->w;
		pm->_31 = -pl->c*lgt->x;		pm->_32 = -pl->c*lgt->y;		pm->_33 = -pl->c*lgt->z + d;	pm->_34 = -pl->c*lgt->w;
		pm->_41 = -pl->d*lgt->x;		pm->_42 = -pl->d*lgt->y;		pm->_43 = -pl->d*lgt->z;		pm->_44 = -pl->d*lgt->w + d;
	}
}

// 아파잉
void k_mat4_affine(kMat4* pm, const kVec3* scl, const kVec3* rotcenter, const kQuat* rot, const kVec3* loc)
{
	kMat4 m1, m2, m3, m4, m5, p1, p2, p3;

	if (scl)
		k_mat4_scl(&m1, scl->x, scl->y, scl->z);
	else
		k_mat4_identify(&m1);

	if (rotcenter)
	{
		k_mat4_loc(&m2, -rotcenter->x, -rotcenter->y, -rotcenter->z);
		k_mat4_loc(&m4, rotcenter->x, rotcenter->y, rotcenter->z);
	}
	else
	{
		k_mat4_identify(&m2);
		k_mat4_identify(&m4);
	}

	if (rot)
		k_mat4_rot_quat(&m3, rot);
	else
		k_mat4_identify(&m3);

	if (loc)
		k_mat4_loc(&m5, loc->x, loc->y, loc->z);
	else
		k_mat4_identify(&m5);

	k_mat4_mul(&p1, &m1, &m2);
	k_mat4_mul(&p2, &p1, &m3);
	k_mat4_mul(&p3, &p2, &m4);
	k_mat4_mul(pm, &p3, &m5);
}

// 합쳐
void k_mat4_interpolate(kMat4* pm, const kMat4* left, const kMat4* right, float f)
{
	float d = 1.0f - f;
	pm->_11 = left->_11*d + right->_11*f;
	pm->_12 = left->_12*d + right->_12*f;
	pm->_13 = left->_13*d + right->_13*f;
	pm->_14 = left->_14*d + right->_14*f;
	pm->_21 = left->_21*d + right->_21*f;
	pm->_22 = left->_22*d + right->_22*f;
	pm->_23 = left->_23*d + right->_23*f;
	pm->_24 = left->_24*d + right->_24*f;
	pm->_31 = left->_31*d + right->_31*f;
	pm->_32 = left->_32*d + right->_32*f;
	pm->_33 = left->_33*d + right->_33*f;
	pm->_34 = left->_34*d + right->_34*f;
	pm->_41 = left->_41*d + right->_41*f;
	pm->_42 = left->_42*d + right->_42*f;
	pm->_43 = left->_43*d + right->_43*f;
	pm->_44 = left->_44*d + right->_44*f;
}

// 트랜스폼
void k_mat4_trfm(kMat4* m, const kVec3* loc, const kQuat* rot, const kVec3* scl)
{
	float* f = m->_m;
	k_mat4_rot_quat(m, rot);
	f[0] += loc->x*f[3];
	f[1] += loc->y*f[3];
	f[2] += loc->z*f[3];
	f[4] += loc->x*f[7];
	f[5] += loc->y*f[7];
	f[6] += loc->z*f[7];
	f[8] += loc->x*f[11];
	f[9] += loc->y*f[11];
	f[10] += loc->z*f[11];
	f[12] += loc->x*f[15];
	f[13] += loc->y*f[15];
	f[14] += loc->z*f[15];
	f[0] *= scl->x;
	f[1] *= scl->x;
	f[2] *= scl->x;
	f[3] *= scl->x;
	f[4] *= scl->y;
	f[5] *= scl->y;
	f[6] *= scl->y;
	f[7] *= scl->y;
	f[8] *= scl->z;
	f[9] *= scl->z;
	f[10] *= scl->z;
	f[11] *= scl->z;
}

// 트랜스폼, 회전벡터
void k_mat4_trfm_vec(kMat4* m, const kVec3* loc, const kVec3* rot, const kVec3* scl)
{
	float* f = m->_m;
	k_mat4_rot_vec(m, rot);
	f[0] += loc->x*f[3];
	f[1] += loc->y*f[3];
	f[2] += loc->z*f[3];
	f[4] += loc->x*f[7];
	f[5] += loc->y*f[7];
	f[6] += loc->z*f[7];
	f[8] += loc->x*f[11];
	f[9] += loc->y*f[11];
	f[10] += loc->z*f[11];
	f[12] += loc->x*f[15];
	f[13] += loc->y*f[15];
	f[14] += loc->z*f[15];
	f[0] *= scl->x;
	f[1] *= scl->x;
	f[2] *= scl->x;
	f[3] *= scl->x;
	f[4] *= scl->y;
	f[5] *= scl->y;
	f[6] *= scl->y;
	f[7] *= scl->y;
	f[8] *= scl->z;
	f[9] *= scl->z;
	f[10] *= scl->z;
	f[11] *= scl->z;
}


//////////////////////////////////////////////////////////////////////////
// 면

// 단위 면적
void k_pln_norm(kPln* pp, const kPln* p)
{
#ifdef SB_MATH_ASM
	__asm
	{
		mov		eax, p;
		movss	xmm0, [eax];
		movss	xmm1, [eax + 4];
		movss	xmm2, [eax + 8];
		movss	xmm3, [eax + 12];
		movaps	xmm4, xmm0;
		movaps	xmm5, xmm1;
		movaps	xmm6, xmm2;
		movaps	xmm7, xmm3;
		mulss	xmm0, xmm0;
		mulss	xmm1, xmm1;
		mulss	xmm2, xmm2;
		mulss	xmm3, xmm3;
		addss	xmm0, xmm1;
		addss	xmm0, xmm2;
		addss	xmm0, xmm3;

		mov		ecx, pp;
		comiss	xmm0, _k_mt_float_zero;
		jae		pos_calc;

		xor		eax, eax;
		mov[ecx], eax;
		mov[ecx + 4], eax;
		mov[ecx + 8], eax;
		mov[ecx + 12], eax;
		jmp		pos_end;

pos_calc:;
		rsqrtss	xmm0, xmm0;
		mulss	xmm4, xmm0;
		mulss	xmm5, xmm0;
		mulss	xmm6, xmm0;
		mulss	xmm7, xmm0;
		movss[ecx], xmm4;
		movss[ecx + 4], xmm5;
		movss[ecx + 8], xmm6;
		movss[ecx + 12], xmm7;

pos_end:;
	}
#else
	float f=k_sqrt(p->a*p->a + p->b*p->b + p->c*p->c);
	if (f>0.0f) f=1.0f/f;
	pp->a=p->a*f;
	pp->b=p->b*f;
	pp->c=p->c*f;
	pp->d=p->d*f;
#endif
}

// 역 노멀
void k_pln_revnorm(kPln* pp, const kPln* p)
{
	float n = -1.0f / k_sqrt(p->a*p->a + p->b*p->b + p->c*p->c);
	pp->a = p->a*n;
	pp->b = p->b*n;
	pp->c = p->c*n;
	pp->d = p->d*n;
}

// 변환
void k_pln_trfm(kPln* pp, const kPln* left, const kMat4* right)
{
#if 0
	k_pln_set(pp, 
		left->a*right->_11 + left->b*right->_21 + left->c*right->_31 + left->d*right->_41,
		left->a*right->_12 + left->b*right->_22 + left->c*right->_32 + left->d*right->_42,
		left->a*right->_13 + left->b*right->_23 + left->c*right->_33 + left->d*right->_43,
		left->a*right->_14 + left->b*right->_24 + left->c*right->_34 + left->d*right->_44);
#else
	kVec3 v, n, s;

	k_vec3_mag(&v, (const kVec3*)left, -left->d);
	k_vec3_trfm_coord(&v, &v, right);
	k_vec3_norm(&n, (const kVec3*)left);
	k_vec3_set(&s, right->_11, right->_22, right->_33);

	if (!k_eq(s.x, 0.0f) && !k_eq(s.y, 0.0f) && !k_eq(s.z, 0.0f) &&
		(k_eq(s.x, 1.0f) || k_eq(s.y, 1.0f) || k_eq(s.z, 1.0f)))
	{
		n.x = n.x / (s.x*s.x);
		n.y = n.y / (s.y*s.y);
		n.z = n.z / (s.z*s.z);
	}

	k_vec3_trfm_norm(&n, &n, right);
	k_vec3_norm((kVec3*)pp, &n);
	pp->d = -k_vec3_dot(&v, (const kVec3*)pp);
#endif
}

void k_pln_from_points(kPln* pp, const kVec3* v1, const kVec3* v2, const kVec3* v3)
{
	kVec3 t0, t1, t2;
	k_vec3_sub(&t0, v2, v1);
	k_vec3_sub(&t1, v3, v2);
	k_vec3_cross(&t2, &t0, &t1);
	k_vec3_norm(&t2, &t2);
	k_pln_set(pp, t2.x, t2.y, t2.z, -k_vec3_dot(v1, &t2));
}

// 면 검출
kcham k_pln_test_intersect_pln(const kPln* p, kVec3* loc, kVec3* dir, const kPln* o)
{
	float f0, f1, f2, det;
	float inv, fa, fb;

	f0 = k_vec3_len((const kVec3*)p);
	f1 = k_vec3_len((const kVec3*)o);
	f2 = k_vec3_dot((const kVec3*)p, (const kVec3*)o);
	det = f0*f1 - f2*f2;

	if (k_abs(det) < KM_EPSILON)
		return FALSE;

	inv = 1.0f / det;
	fa = (f1*-p->d + f2*o->d) * inv;
	fb = (f0*-o->d + f2*p->d) * inv;

	k_vec3_cross(dir, (const kVec3*)p, (const kVec3*)o);
	k_vec3_set(loc, p->a*fa + o->a*fb, p->b*fa + o->b*fb, p->c*fa + o->c*fb);

	return TRUE;
}

// 선 충돌
float k_pln_intersect_line(const kPln* p, const kVec3* v1, const kVec3* v2)
{
	kVec3 t;
	float f;
	k_vec3_sub(&t, v2, v1);
	f = 1.0f / k_vec3_dot((const kVec3*)p, &t);
	return -(k_vec3_dot((const kVec3*)p, v1) + p->d)*f;
}

// 면 충돌
kcham k_pln_intersect_plane(const kPln* p, const kPln* o1, const kPln* o2)
{
	kVec3 v, dir, loc;
	return (k_pln_test_intersect_pln(p, &loc, &dir, o1)) ? k_vec3_intersect_line(&v, o2, &loc, &dir) : FALSE;
}


//////////////////////////////////////////////////////////////////////////
// 선

// 구 검출
kcham k_ln3_intersect_sphere(const kLn3* p, const kVec3* org, float rad, float* dist)
{
	kVec3 t, v;
	float c, z, d;

	k_vec3_sub(&t, org, &p->begin);
	c = k_vec3_len(&t);

	k_ln3_vec(p, &v);
	k_vec3_norm(&v, &v);
	z = k_vec3_dot(&t, &v);

	d = rad*rad - (c*c - z*z);

	if (d < 0.0f)
		return FALSE;

	if (dist)
		*dist = z - k_sqrt(d);
	return TRUE;
}


//////////////////////////////////////////////////////////////////////////
// 정렬된 행렬

#if 0
kAliMat4* k_mat4_align_alloc(ksize_t n)
{
	kbyte* p=k_new_zero(n*sizeof(kAliMat4)+16, kbyte);
	kbyte o=(kbyte)(16-((kuintptr)p&15));
	p+=o;
	p[-1]=o;
	return (kAliMat4*)p;
}

kAliMat4* k_mat4_align_allocf(ksize_t n)
{
	kbyte* p=k_new(n*sizeof(kAliMat4)+16, kbyte);
	kbyte o=(kbyte)(16-((kuintptr)p&15));
	p+=o;
	p[-1]=o;
	return (kAliMat4*)p;
}

void k_mat4_align_free(kAliMat4* m)
{
	if (m)
	{
		kbyte* p=(kbyte*)m;
		p-=p[-1];
		k_delete(p);
	}
}
#endif

